/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_GNSS.c
 *
 *  Created on: 2022年7月17日
 *      Author: 64435
 */

#include "LOS_GNSS.h"
#include <LOS_RealTime.h>
#include "LOS_SYS.h"

LOS_GNSSSTA_t STA_GNSS;

uint8_t GNSS_CMD_TimePolling[3] = {0x00,0x10,0x01};    //时间数据轮询
uint8_t GNSS_CMD_StatusPolling[3] = {0x00,0x10,0x02};  //基本状态轮询
uint8_t GNSS_CMD_OriginPolling[3] = {0x00,0x10,0x03};  //原始观测量轮询
uint8_t GNSS_CMD_Reset[4] = {0x0D,0x00,0xAA,0xAA};     //GNSS复位

uint8_t BUF_GNSS_OriginData[511];  //原始观测量：前10字节是包头
uint8_t TMPACK_GNSSBasic[161];     //基本遥测
uint8_t TMPACK_GNSSRadio[64];      //定位广播
uint8_t TMPACK_GNSSEphemeris[114]; //星历数据

uint8_t FLAG_GNSS_OriginData = 0;    //原始观测量接收完成标志位

// uint8_t BUF_GNSS_Star[501];
// uint8_t FLAG_GNSS_Star = 0;    //原始观测量接收完成标志位

void LOS_GNSS_CMD(uint8_t* cmd)
{
    LOS_SNP_t SNP_CMD;    //首部配置结构体
    uint8_t FP_CMD[8];    //首部内容
    uint8_t rec = 0x02;
    uint8_t len = 3;

    if(cmd[0]==0x0D)
    {
        rec = 0x00;
        len = 4;
    }

    SNP_CMD.SNP_dest = nID_GNSS;        //GNSS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = rec;              //内容识别码
    SNP_CMD.SNP_len = 8 + len;          //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);

    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_GNSS, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_GNSS, 0XFF), cmd, len);
}

void LOS_CAN_GNSSCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;
    uint8_t sum_check = 0;
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t canmsgrec = 0xFF;       //消息类型（对应首部的rec）
    static uint32_t CNT_OriginData = 0;    //原始观测量接收计数器
    int32_t sbuf = 0;
    uint32_t ubuf = 0;
    static uint8_t timecheckcnt = 59;        //粗校时计数器


    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
        }
        else 
        {
           canerrflag = 1; 
        }

        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            switch(can_re->msg[6])
            {
                case 0xF0:  //GNSS时间轮询
                canmsgrec = 0;
                break;

                case 0xF1:  //GNSS基本状态轮询
                canmsgrec = 1;
                break;

                case 0xF2:  //GNSS定位数据广播
                canmsgrec = 2;
                STA_GNSS.TMCNT_RADIO++; //广播帧计数
                break;

                case 0xF3:  //GNSS原始观测量轮询
                canmsgrec = 3;
                break;

                default:
                canmsgrec = 0xFF;
                break;
            }
        }
    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec==0)    //时间
            {
                memcpy(&LOS_Time_Sec,&(can_re->msg[0]),4);
                LOS_Time_Sec++;   //GNSS时间轮询比实际时间慢一秒，需要补回
                memcpy(&LOS_Time_MilSec,&(can_re->msg[4]),2);
                STA_GNSS.GNSS_Time = (LOS_Time_Sec - 1) + 0.001 * (float)LOS_Time_MilSec;
            }
            else if (canmsgrec==1)   //基本遥测
            {
                memcpy(&TMPACK_GNSSBasic[154], &can_re->msg[0], 7);
            }
            else if (canmsgrec == 2) //定位数据广播
            {
                if (can_re->msg[0] == 0xAA && can_re->msg[1] == 0xAA && can_re->msg[2] == 0xAA)
                {
                }
                else
                {
                    memcpy(&ubuf, &(can_re->msg[0]), 4);
                    memcpy(&STA_GNSS.orbit_o[5], &(can_re->msg[0]), 4);
                    STA_GNSS.orbit[5] = 3.1415926 * (float)ubuf * (931.322574615478E-12);    //平近点角
                    memcpy(&STA_GNSS.pos_flag, &(can_re->msg[4]), 1);
                    STA_GNSS.pos_type = STA_GNSS.pos_flag;
                    STA_GNSS.pos_flag = STA_GNSS.pos_flag >> 4;
                    if (STA_GNSS.pos_flag == 9)  //定位中
                    {
                        STA_GNSS.pos_flag = 1;
                    }
                    else //非定位
                    {
                        STA_GNSS.pos_flag = 0;
                    }
                    STA_GNSS.pos_type = STA_GNSS.pos_type << 6;
                    STA_GNSS.pos_type = STA_GNSS.pos_type >> 6;  //定位类型
                    memcpy(&TMPACK_GNSSRadio[58], &can_re->msg[0], 8);
                }
                
            }
            else if (canmsgrec==3)
            {
                memcpy(&BUF_GNSS_OriginData[10 + CNT_OriginData],&(can_re->msg[0]),5);
                CNT_OriginData = 0;
                FLAG_GNSS_OriginData = 1;
            }
        }
        
        canerrflag = 0;   //最后一帧时清除标志位
        canmsgrec = 0xFF; //同上
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec==0)
            {

            }
            else if (canmsgrec==1)
            {
                if (id_filter[3]==0x01)      //基本状态轮询
                {
                    STA_GNSS.Flag_D = can_re->msg[4];
                    if ((STA_GNSS.Flag_D & 0xC0) == 0)
                    {
                        STA_GNSS.Flag_D = 0;
                    }
                    else if ((STA_GNSS.Flag_D & 0x30) != 0)
                    {
                        STA_GNSS.Flag_D = 0;
                    }
                    else
                    {
                        STA_GNSS.Flag_D = 1;
                    }

                    memcpy(&TMPACK_GNSSBasic[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_GNSSBasic[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_GNSSBasic[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_GNSSBasic[10], &can_re->msg[0], 8);
                }
                else
                {
                    memcpy(&TMPACK_GNSSBasic[10 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }

            }
            else if (canmsgrec == 2)   //定位数据广播
            {
                if (id_filter[3]==0x01)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        timecheckcnt ++;
                        memcpy(&STA_GNSS.sec, &(can_re->msg[0]), 4);
                        STA_GNSS.GNSS_Time = (float)STA_GNSS.sec + 1.1;
                        if (timecheckcnt == 60)
                        {
                            LOS_Time_Double = STA_GNSS.GNSS_Time;
                            LOS_Time_Sec = STA_GNSS.sec + 1;
                            LOS_Time_MilSec = 100;
                            timecheckcnt = 0;
                        }
                        memcpy(&sbuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[0], &(can_re->msg[4]), 4);
                        STA_GNSS.pos_vel[0] = 0.1 * (float)sbuf;

                        memcpy(&TMPACK_GNSSRadio[0], &LOS_Time_SecFromBoot, 4);
                        memcpy(&TMPACK_GNSSRadio[4], &LOS_Time_MilSec, 2);
                        memcpy(&TMPACK_GNSSRadio[6], &LOS_Time_Sec, 4);
                        memcpy(&TMPACK_GNSSRadio[10], &can_re->msg[0], 8);
                    }

                }
                else if (id_filter[3]==0x02)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        memcpy(&sbuf, &(can_re->msg[0]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[1], &(can_re->msg[0]), 4);
                        STA_GNSS.pos_vel[1] = 0.1 * (float)sbuf;
                        memcpy(&sbuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[2], &(can_re->msg[4]), 4);
                        STA_GNSS.pos_vel[2] = 0.1 * (float)sbuf;
                        memcpy(&TMPACK_GNSSRadio[18], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x03)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        memcpy(&sbuf, &(can_re->msg[0]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[3], &(can_re->msg[0]), 4);
                        STA_GNSS.pos_vel[3] = 0.01 * (float)sbuf;
                        memcpy(&sbuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[4], &(can_re->msg[4]), 4);
                        STA_GNSS.pos_vel[4] = 0.01 * (float)sbuf;
                        memcpy(&TMPACK_GNSSRadio[26], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x04)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        memcpy(&sbuf, &(can_re->msg[0]), 4);
                        memcpy(&STA_GNSS.pos_vel_o[5], &(can_re->msg[0]), 4);
                        STA_GNSS.pos_vel[5] = 0.01 * (float)sbuf;
                        memcpy(&ubuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.orbit_o[0], &(can_re->msg[4]), 4);
                        STA_GNSS.orbit[0] = 0.1 * (float)ubuf;     //半长轴
                        memcpy(&TMPACK_GNSSRadio[34], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x05)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        memcpy(&ubuf, &(can_re->msg[0]), 4);
                        memcpy(&STA_GNSS.orbit_o[1], &(can_re->msg[0]), 4);
                        STA_GNSS.orbit[1] = (float)ubuf*3.0*(10e-3)*(0.00000000745);    //偏心率
                        memcpy(&ubuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.orbit_o[2], &(can_re->msg[4]), 4);
                        STA_GNSS.orbit[2] = 3.1415926 * (float)ubuf * (931.322574615478E-12);  //轨道倾角
                        memcpy(&TMPACK_GNSSRadio[42], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x06)
                {
                    if (can_re->msg[0] == 0xAA && can_re->msg[2] == 0xAA && can_re->msg[4] == 0xAA && can_re->msg[6] == 0xAA)
                    {

                    }
                    else
                    {
                        memcpy(&ubuf, &(can_re->msg[0]), 4);
                        memcpy(&STA_GNSS.orbit_o[3], &(can_re->msg[0]), 4);
                        STA_GNSS.orbit[3] = 3.1415926 * (float)ubuf * (931.322574615478E-12);    //升交点赤经
                        memcpy(&ubuf, &(can_re->msg[4]), 4);
                        memcpy(&STA_GNSS.orbit_o[4], &(can_re->msg[4]), 4);
                        STA_GNSS.orbit[4] = 3.1415926 * (float)ubuf * (931.322574615478E-12);    //近地点角距                    
                        memcpy(&TMPACK_GNSSRadio[50], &can_re->msg[0], 8);
                    }
                }

            }
            else if (canmsgrec==3)
            {
                memcpy(&BUF_GNSS_OriginData[10 + CNT_OriginData],&(can_re->msg[0]),8);
                CNT_OriginData += 8;
            }
        }
    }
}


void LOS_GNSS_PWROn()
{
    LOS_EPS_Switch(8,SWITCH_ON);
    STA_GNSS.CMD = 1;
}

void LOS_GNSS_PWROff()
{
    LOS_EPS_Switch(8,SWITCH_OFF);
    STA_GNSS.CMD = 0;
}


